import tkinter as tk
import numpy as np
import random
import math


# 定义方向
DIRECTIONS = [
    (-1, 0),  # 上
    (1, 0),  # 下
    (0, -1),  # 左
    (0, 1),  # 右
    (-1, -1),  # 左上
    (-1, 1),  # 右上
    (1, -1),  # 左下
    (1, 1)  # 右下
]


# 计算欧几里得距离
def distance(p1, p2):
    return np.linalg.norm(np.array(p1) - np.array(p2))


# 计算吸引力函数
def compute_attraction_force(current_pos, goal_pos, attraction_strength):
    dist = distance(current_pos, goal_pos)
    force = attraction_strength / dist if dist != 0 else 0
    direction = np.array(goal_pos) - np.array(current_pos)
    norm_direction = direction / np.linalg.norm(direction)
    return force * norm_direction


# 计算排斥力函数，根据障碍物的半径和比例系数动态调整影响范围
def compute_repulsion_force(current_pos, obstacle_centers, repulsion_strength, repulsion_range_factor):
    total_force = np.array([0.0, 0.0])
    for center, radius in obstacle_centers:
        repulsion_range = radius * repulsion_range_factor  # 排斥力范围为圆形半径的倍数
        dist = distance(current_pos, center)
        if dist < repulsion_range:
            force = repulsion_strength * (1 / dist - 1 / repulsion_range) / (dist ** 2)
            direction = np.array(current_pos) - np.array(center)
            norm_direction = direction / np.linalg.norm(direction)
            total_force += force * norm_direction
    return total_force


# 随机选择一个未访问且无障碍的方向
def find_next_valid_direction(current_pos, grid, visited, force):
    sorted_directions = sorted(DIRECTIONS,
                               key=lambda direction: np.dot(force, np.array(direction) / np.linalg.norm(direction)),
                               reverse=True)

    for direction in sorted_directions:
        new_pos = (current_pos[0] + direction[0], current_pos[1] + direction[1])
        if 0 <= new_pos[0] < grid.shape[0] and 0 <= new_pos[1] < grid.shape[1] and visited[new_pos] == 0 and grid[new_pos] == 0:
            return direction

    return None


# 路径规划主函数
def path_planning(grid, start, goal, attraction_strength, repulsion_strength, repulsion_range_factor, max_steps,
                  obstacle_centers):
    current_pos = start
    path = [start]
    visited = np.zeros_like(grid)
    visited[start] = 1

    for _ in range(max_steps):
        attraction_force = compute_attraction_force(current_pos, goal, attraction_strength)
        repulsion_force = compute_repulsion_force(current_pos, obstacle_centers, repulsion_strength, repulsion_range_factor)
        total_force = attraction_force + repulsion_force

        best_direction = find_next_valid_direction(current_pos, grid, visited, total_force)

        if best_direction is None:
            break

        current_pos = (current_pos[0] + best_direction[0], current_pos[1] + best_direction[1])
        visited[current_pos] = 1

        if distance(current_pos, goal) < 1:
            path.append(goal)
            break

        path.append(current_pos)

    return path


# GUI 类
class PathPlanningApp:
    def __init__(self, root, grid_size=900, cell_size=1):
        self.root = root
        self.grid_size = grid_size
        self.cell_size = cell_size
        self.grid = np.zeros((grid_size, grid_size))
        self.start = None
        self.goal = None

        # 创建主框架
        self.main_frame = tk.Frame(root)
        self.main_frame.pack(fill=tk.BOTH, expand=True)

        # 左侧的画布框架
        self.canvas_frame = tk.Frame(self.main_frame)
        self.canvas_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)

        # 右侧的控件框架
        self.control_frame = tk.Frame(self.main_frame)
        self.control_frame.pack(side=tk.RIGHT, fill=tk.Y)

        # 画布放在左侧
        self.canvas = tk.Canvas(self.canvas_frame, width=grid_size * cell_size, height=grid_size * cell_size)
        self.canvas.pack()

        # 重置按钮放在右侧
        self.reset_button = tk.Button(self.control_frame, text="Reset", command=self.reset_grid)
        self.reset_button.pack(pady=10)

        # 吸引力进度条
        self.attraction_slider = tk.Scale(self.control_frame, from_=0.01, to=1, orient="horizontal", resolution=0.01,
                                          label="Attraction Strength",
                                          command=self.update_path)

        self.attraction_slider.set(0.1)  # 设置默认值为 0.1
        self.attraction_slider.pack(pady=10)

        # 排斥力进度条
        self.repulsion_slider = tk.Scale(self.control_frame, from_=200, to=4000, orient="horizontal", label="Repulsion Strength",
                                         command=self.update_path)
        self.repulsion_slider.set(2000)  # 设置默认值为 2000
        self.repulsion_slider.pack(pady=10)

        # 排斥力范围进度条（作为比例系数）
        self.repulsion_range_slider = tk.Scale(self.control_frame, from_=1, to=5, orient="horizontal", resolution=0.1, label="Repulsion Range Factor",
                                               command=self.update_path)
        self.repulsion_range_slider.set(2)  # 设置默认比例系数为 2
        self.repulsion_range_slider.pack(pady=10)

        self.max_steps = 2000
        self.obstacle_centers = []

        self.generate_random_circles()

        self.canvas.bind("<Button-1>", self.set_start_goal)

    # 判断圆形是否重叠
    def check_circle_overlap(self, center, radius):
        for existing_center, existing_radius in self.obstacle_centers:
            if distance(center, existing_center) < radius + existing_radius:
                return True
        return False

    # 随机生成不重叠的圆形障碍物
    def generate_random_circles(self):
        num_circles = random.randint(5, 15)  # 生成 5 到 15 个圆形障碍物
        for _ in range(num_circles):
            while True:
                radius = random.randint(10, 30)  # 每个圆的半径在 10 到 30 之间
                center_x = random.randint(radius, self.grid_size - radius)
                center_y = random.randint(radius, self.grid_size - radius)
                center = (center_y, center_x)

                if not self.check_circle_overlap(center, radius):
                    self.obstacle_centers.append((center, radius))  # 存储圆心位置和半径
                    break

            # 分层绘制圆的斥力范围
            self.draw_repulsion_zones(center, radius)

            # 填充圆形内部为障碍物
            for y in range(center_y - radius, center_y + radius + 1):
                for x in range(center_x - radius, center_x + radius + 1):
                    if distance((y, x), center) <= radius:
                        if 0 <= x < self.grid_size and 0 <= y < self.grid_size:
                            self.grid[y, x] = 1
                            self.canvas.create_rectangle(
                                x * self.cell_size, y * self.cell_size,
                                (x + 1) * self.cell_size, (y + 1) * self.cell_size,
                                fill="black"
                            )

    # 绘制斥力范围的颜色层
    def draw_repulsion_zones(self, center, radius):
        repulsion_range_factor = self.repulsion_range_slider.get()
        repulsion_range = radius * repulsion_range_factor

        # 将斥力范围分为多个区间，例如每 5 像素为一个区间
        num_zones = 10  # 斥力层数
        step = repulsion_range / num_zones

        for i in range(num_zones):
            zone_radius = radius + (i + 1) * step
            # 颜色渐变：内圈颜色更深，外圈颜色更浅
            color_value = int(255 - 25 * (num_zones - i))  # 颜色值从深到浅
            color = "#%02x%02x%02x" % (color_value, color_value, color_value)  # 使用灰度色

            self.canvas.create_oval(
                (center[1] - zone_radius) * self.cell_size, (center[0] - zone_radius) * self.cell_size,
                (center[1] + zone_radius) * self.cell_size, (center[0] + zone_radius) * self.cell_size,
                outline=color
            )

    # 设置起点或终点
    def set_start_goal(self, event):
        x, y = event.x // self.cell_size, event.y // self.cell_size
        if self.grid[y, x] == 0:
            if not self.start:
                self.start = (y, x)
                self.canvas.create_rectangle(
                    x * self.cell_size, y * self.cell_size,
                    (x + 1) * self.cell_size, (y + 1) * self.cell_size,
                    fill="lime", outline="red", width=8  # 使用鲜亮的颜色和加粗边框
                )
                # 添加起点文字，稍微偏下
                self.canvas.create_text(
                    (x + 0.5) * self.cell_size, (y + 17) * self.cell_size,  # y 增加了偏移
                    text="Start", fill="black", font=("Arial", 12, "bold")
                )
            elif not self.goal and (y, x) != self.start:
                self.goal = (y, x)
                self.canvas.create_oval(
                    x * self.cell_size, y * self.cell_size,
                    (x + 1) * self.cell_size, (y + 1) * self.cell_size,
                    fill="orange", outline="pink", width=8  # 使用鲜亮的颜色和加粗边框
                )
                # 添加终点文字，稍微偏下
                self.canvas.create_text(
                    (x + 0.5) * self.cell_size, (y + 17) * self.cell_size,  # y 增加了偏移
                    text="Goal", fill="black", font=("Arial", 12, "bold")
                )
                self.run_path_planning()

    # 执行路径规划
    def run_path_planning(self):
        if self.start and self.goal:
            attraction_strength = self.attraction_slider.get()
            repulsion_strength = self.repulsion_slider.get()
            repulsion_range_factor = self.repulsion_range_slider.get()

            path = path_planning(self.grid, self.start, self.goal, attraction_strength, repulsion_strength,
                                 repulsion_range_factor, self.max_steps, self.obstacle_centers)

            # 用 "path" 标签绘制路径
            for pos in path:
                x, y = pos[1], pos[0]
                self.canvas.create_rectangle(
                    x * self.cell_size, y * self.cell_size,
                    (x + 1) * self.cell_size, (y + 1) * self.cell_size,
                    fill="blue", tags="path"
                )

    # 更新路径
    def update_path(self, event=None):
        if self.start and self.goal:
            self.canvas.delete("path")  # 清除之前的路径
            self.run_path_planning()

    # 重置网格
    def reset_grid(self):
        self.grid = np.zeros((self.grid_size, self.grid_size))
        self.start = None
        self.goal = None
        self.obstacle_centers = []
        self.canvas.delete("all")
        self.attraction_slider.set(0.1)  # 重置吸引力
        self.repulsion_slider.set(2000)  # 重置排斥力
        self.repulsion_range_slider.set(2)  # 重置排斥力范围系数
        self.generate_random_circles()


# 创建Tkinter应用并启动主循环
root = tk.Tk()
app = PathPlanningApp(root)
root.mainloop()
